﻿#include "algo_approximate.h"
#include <Dense>
#include <Sparse>

Handle(CAGD::GeomBezierCurve) CAGD::Approximate::BezierCurve(int n, const std::vector<BPnt> &points)
{
    int N = points.size() - 1;
    Eigen::MatrixXd M(N + n, n + 1);
    Eigen::MatrixXd Q(N + n, 3);

    M.setZero();
    Q.setZero();

    // 生成均匀参数（这里可以改为通过外部传入参数值）
    std::vector<double> para;
    for (int i = 0; i < N + 1; i++)
        para.push_back(i * 1.0 / N);

    // 填系数矩阵
    for (int i = 0; i < N + 1; i++)
        for (int j = 0; j < n + 1; j++)
            M(i, j) = bernstein(n, j, para[i]);

    for (int i = 0; i < n - 1; i++)
    {
        M(i + N + 1, i) = 1;
        M(i + N + 1, i + 1) = -2;
        M(i + N + 1, i + 2) = 1;
    }

    // 填充 Q
    for (int i = 0; i < N + 1; i++)
    {
        Q(i, 0) = points[i].X();
        Q(i, 1) = points[i].Y();
        Q(i, 2) = points[i].Z();
    }
    for (int i = 0; i < n - 1; i++)
    {
        Q(i + N + 1, 0) = 0;
        Q(i + N + 1, 1) = 0;
        Q(i + N + 1, 2) = 0;
    }

    // 求解
    Eigen::MatrixXd P = M.fullPivHouseholderQr().solve(Q);
    std::vector<BPnt> cpts;
    for (int i = 0; i < n + 1; i++)
        cpts.push_back(BPnt(P(i, 0), P(i, 1), P(i, 2)));

    return Handle(GeomBezierCurve)(new GeomBezierCurve(cpts));
}

Handle(CAGD::GeomBSplineCurve) CAGD::Approximate::BSplineCurve(int deg, int N, const std::vector<BPnt> &points)
{
    // N 个控制点
    int n = N - 1;
    int m = points.size() - 1;

    // 生成均匀参数（这里可以改为通过外部传入参数值）
    std::vector<double> para;
    for (int i = 0; i < m + 1; i++)
        para.push_back(i * 1.0 / m);

    // 生成均匀节点向量
    std::vector<double> tau;
    for (int i = 0; i < deg + 1; i++)
        tau.push_back(0);
    for (int i = deg + 1; i < n + 1; i++)
        tau.push_back(1.0 * (i - deg) / (n + 1 - deg));
    for (int i = n + 1; i < n + deg + 2; i++)
        tau.push_back(1);

    // 填充系数矩阵
    std::vector<Eigen::Triplet<double>> trip;
    for (int i = 0; i < m + 1; i++)
    {
        int span = Spline::FindSpan(para[i], deg, tau);
        auto N = Spline::Basis(span, para[i], deg, tau);
        for (int j = 0; j < deg + 1; j++)
            trip.push_back({i, span - deg + j, N[j]});
    }
    Eigen::SparseMatrix<double> A(m + 1, n + 1);
    A.setFromTriplets(trip.begin(), trip.end());

    // 填充 Q
    Eigen::MatrixXd Q(m + 1, 3);
    for (int i = 0; i < m + 1; i++)
    {
        Q(i, 0) = points[i].X();
        Q(i, 1) = points[i].Y();
        Q(i, 2) = points[i].Z();
    }

    // 求解最小二乘系统
    Eigen::SparseQR<Eigen::SparseMatrix<double>, Eigen::COLAMDOrdering<int>> solver;

    A.makeCompressed();
    solver.compute(A);

    auto P = solver.solve(Q);
    std::vector<BPnt> cpts;
    for (int i = 0; i < n + 1; i++)
        cpts.push_back(BPnt(P(i, 0), P(i, 1), P(i, 2)));

    return Handle(GeomBSplineCurve)(new GeomBSplineCurve(tau, cpts));
}
